#include "emodbus.h"

#define E_MODBUS_RX {}
#define E_MODBUS_TX {}
#define E_MODBUS_DEBUG 1

//接收发送缓存
uint8_t eModbusRXBuff[E_MODBUS_RX_BUFF_SIZE] = {0};
uint8_t eModbusTXBuff[E_MODBUS_TX_BUFF_SIZE] = {0};

//读写内存定义
uint16_t eModbusRReg[E_MODBUS_R_REG_SIZE] = {0};
uint16_t eModbusRWReg[E_MODBUS_RW_REG_SIZE] = {0};

//从设备配置 站号读写寄存器地址定义等
_EModbusDec EModbusDec = {
        0x11,//17

        0x190,//400
        E_MODBUS_R_REG_SIZE,
        eModbusRReg,

        0x1f4, //500
        E_MODBUS_RW_REG_SIZE,
        eModbusRWReg,

};


//公有数据索引读部分
#define INDEX_RW_REG_LENGTH  4


static uint16_t pop_top_index = 0x00;
static uint16_t push_last_index = 0x00;

void pop_top_index_clear() {
    pop_top_index = 0;
}

void push_last_index_clear() {
    push_last_index = 0;
}

uint8_t pop_top(uint8_t *data_buff) {
    return data_buff[pop_top_index++];
}

uint8_t push_last(uint8_t *data_buff, uint8_t data) {
    data_buff[push_last_index] = data;
    push_last_index++;
    return data;
}


/*
 * 检验数据中 站号 地址 长度等是否长度等是否正确 并提取数据格式后续使用
 * 站号
 * 功能码
 */

_EModbusDataDec EModbusDataDec = {0};


void emodbus_dec_clear() {
    //清除历史数据
    EModbusDataDec.slave_id = 0x00;
    EModbusDataDec.func_code = 0x00;
    EModbusDataDec.read_write_reg_start = 0x00;
    EModbusDataDec.read_write_reg_length = 0x00;
    EModbusDataDec.read_write_data_p = 0x00;
    EModbusDataDec.func_private_code1 = 0x00;
    EModbusDataDec.func_private_code2 = 0x00;
    EModbusDataDec.crc = 0x00;
    EModbusDataDec.data_length = 0x00;

}


void show_r_w_reg() {
    uint16_t index;
    printf("\r\nR REG Addr Start:%2x,Size:%2x\r\n",EModbusDec.read_reg_start_addr,EModbusDec.read_reg_length);
    for(index=0;index<EModbusDec.read_reg_length;index++){
        if(index==0){
            printf("%04x",EModbusDec.read_reg_start_addr+index);
        } else if(index%4==0){
            printf("\r\n%04x",EModbusDec.read_reg_start_addr+index);
        }
        printf(" %04x",eModbusRReg[index]);

    }

    printf("\r\nRW REG Addr Start:%2x,Size:%2x\r\n",EModbusDec.read_write_reg_start_addr,EModbusDec.read_write_reg_length);
    for(index=0;index<EModbusDec.read_write_reg_length;index++){
        if(index==0){
            printf("%04x",EModbusDec.read_write_reg_start_addr+index);
        } else if(index%4==0){
            printf("\r\n%04x",EModbusDec.read_write_reg_start_addr+index);
        }
        printf(" %04x",eModbusRWReg[index]);

    }
    printf("\r\n");
}





//判读数据长度和地址是否正常
uint8_t verify_emodbus_rw_addr() {
    uint8_t is_pass = TRUE;
    if (EModbusDataDec.func_code == E_MODBUS_WRITE_FUNC_CODE) {
        is_pass = (
                EModbusDataDec.read_write_reg_length < E_MODBUS_RW_REG_SIZE &&
                EModbusDataDec.read_write_reg_start >= EModbusDec.read_write_reg_start_addr &&
                EModbusDataDec.read_write_reg_start < EModbusDec.read_write_reg_start_addr + E_MODBUS_RW_REG_SIZE &&
                EModbusDataDec.read_write_reg_start + EModbusDataDec.read_write_reg_length <
                EModbusDec.read_write_reg_start_addr + E_MODBUS_RW_REG_SIZE
        );
    } else if (EModbusDataDec.func_code == E_MODBUS_READ_FUNC_CODE) {
        is_pass = (
                          EModbusDataDec.read_write_reg_length < E_MODBUS_R_REG_SIZE &&
                          EModbusDataDec.read_write_reg_start >= EModbusDec.read_reg_start_addr &&
                          EModbusDataDec.read_write_reg_start < EModbusDec.read_reg_start_addr + E_MODBUS_R_REG_SIZE &&
                          EModbusDataDec.read_write_reg_start + EModbusDataDec.read_write_reg_length <
                          EModbusDec.read_reg_start_addr + E_MODBUS_R_REG_SIZE
                  ) || (
                          EModbusDataDec.read_write_reg_length < E_MODBUS_RW_REG_SIZE &&
                          EModbusDataDec.read_write_reg_start >= EModbusDec.read_write_reg_start_addr &&
                          EModbusDataDec.read_write_reg_start <
                          EModbusDec.read_write_reg_start_addr + E_MODBUS_RW_REG_SIZE &&
                          EModbusDataDec.read_write_reg_start + EModbusDataDec.read_write_reg_length <
                          EModbusDec.read_write_reg_start_addr + E_MODBUS_RW_REG_SIZE
                  );
    }

    return is_pass;
}


uint8_t verify_emodbus_rx_data(uint8_t *data_buff) {
    uint8_t is_pass = TRUE;
    uint16_t crc_clc;

    emodbus_dec_clear();
    pop_top_index_clear();

    EModbusDataDec.slave_id = pop_top(data_buff);
    if (EModbusDataDec.slave_id == EModbusDec.slave_id) {
        EModbusDataDec.func_code = pop_top(data_buff);

        //处理功能码
        if (EModbusDataDec.func_code == E_MODBUS_READ_FUNC_CODE ||
            EModbusDataDec.func_code == E_MODBUS_WRITE_FUNC_CODE) {//读写功能码
            //匹配地址
            EModbusDataDec.read_write_reg_start = pop_top(data_buff) << 8 | pop_top(data_buff);
            EModbusDataDec.read_write_reg_length = pop_top(data_buff) << 8 | pop_top(data_buff);

            if (EModbusDataDec.func_code == E_MODBUS_READ_FUNC_CODE) {

                EModbusDataDec.crc = pop_top(data_buff) << 8 | pop_top(data_buff);
                crc_clc = usMBCRC16(data_buff, pop_top_index - 2);
                if (crc_clc != EModbusDataDec.crc) {
                    is_pass = FALSE;
                }

            } else {

                EModbusDataDec.read_write_data_p = &data_buff[pop_top_index];

                pop_top_index = INDEX_RW_REG_LENGTH + 2 + EModbusDataDec.read_write_reg_length * 2;
                //防止指针越界
                pop_top_index = pop_top_index > E_MODBUS_RX_BUFF_SIZE ? E_MODBUS_RX_BUFF_SIZE - 2 : pop_top_index;
                EModbusDataDec.crc = pop_top(data_buff) << 8 | pop_top(data_buff);
                //检验CRC
                crc_clc = usMBCRC16(data_buff, pop_top_index - 2);
                if (crc_clc != EModbusDataDec.crc) {
                    is_pass = FALSE;
                }
            }

            //判定地址是否是只读寄存器
            if (!verify_emodbus_rw_addr()) {
                is_pass = FALSE;
            }
        } else if (EModbusDataDec.func_code == E_MODBUS_PRAViTE_FUNC_CODE) {
            EModbusDataDec.func_private_code1 = pop_top(data_buff);
            EModbusDataDec.func_private_code2 = pop_top(data_buff);

            EModbusDataDec.crc = pop_top(data_buff) << 8 | pop_top(data_buff);

            //检验CRC
            crc_clc = usMBCRC16(data_buff, pop_top_index - 2);
            if (crc_clc != EModbusDataDec.crc) {
                is_pass = FALSE;
            }

        } else {//功能码未匹配
            is_pass = FALSE;
        }
    } else {//站号不匹配
        is_pass = FALSE;
    }
    pop_top_index_clear();
    return is_pass;
}

void show_emodbus_tx_data(uint16_t length) {
    uint16_t index = 0;
    printf("\r\n");
    printf("Tx Data:");
    for (; index < length; index++) {
        printf("%2x ", eModbusTXBuff[index]);
    }
    printf("\r\n");

}

void clear_tx_data() {
    uint16_t index = 0;
    for (; index < E_MODBUS_TX_BUFF_SIZE; index++) {
        eModbusTXBuff[index] = 0x00;
    }
}

void generate_tx_data() {

    uint16_t offset;
    uint16_t index ;
    uint16_t crc;

    //清除发送缓冲区
    clear_tx_data();
    push_last_index_clear();

    //生成回复码 地址 命令码 长度 crc
    //数据长度
    EModbusDataDec.data_length = EModbusDataDec.read_write_reg_length;

    push_last(eModbusTXBuff, EModbusDec.slave_id);
    push_last(eModbusTXBuff, EModbusDataDec.func_code);

    if (EModbusDataDec.func_code == E_MODBUS_WRITE_FUNC_CODE ||
        EModbusDataDec.func_code == E_MODBUS_PRAViTE_FUNC_CODE) {
        push_last(eModbusTXBuff, (EModbusDataDec.data_length >> 8) & 0xff);
        crc = usMBCRC16(eModbusTXBuff, push_last_index);
        push_last(eModbusTXBuff, (crc >> 8) & 0xff);
        push_last(eModbusTXBuff, crc & 0xff);
    } else if (EModbusDataDec.func_code == E_MODBUS_READ_FUNC_CODE) {
        push_last(eModbusTXBuff, EModbusDataDec.data_length & 0xff);
        //产生读取内存数据 依据不同的地址
        if (
                EModbusDataDec.read_write_reg_start >= EModbusDec.read_reg_start_addr &&
                EModbusDataDec.read_write_reg_start < EModbusDec.read_reg_start_addr + E_MODBUS_R_REG_SIZE
        ) {
            offset = EModbusDataDec.read_write_reg_start - EModbusDec.read_reg_start_addr;
            for (index = offset; index < (offset + EModbusDataDec.read_write_reg_length); index++) {
                push_last(eModbusTXBuff, (eModbusRReg[index] >> 8) & 0xff);
                push_last(eModbusTXBuff, eModbusRReg[index] & 0xff);
            }
        } else if (
                EModbusDataDec.read_write_reg_start >= EModbusDec.read_write_reg_start_addr &&
                EModbusDataDec.read_write_reg_start < EModbusDec.read_write_reg_start_addr + E_MODBUS_RW_REG_SIZE
        ) {
            offset = EModbusDataDec.read_write_reg_start - EModbusDec.read_write_reg_start_addr;
            for (index = offset; index < (offset + EModbusDataDec.read_write_reg_length); index++) {
                push_last(eModbusTXBuff, (eModbusRWReg[index] >> 8) & 0xff);
                push_last(eModbusTXBuff, eModbusRWReg[index] & 0xff);
            }
        }


        crc = usMBCRC16(eModbusTXBuff, push_last_index);
        push_last(eModbusTXBuff, (crc >> 8) & 0xff);
        push_last(eModbusTXBuff, crc & 0xff);
    }

    //ToDo 此处触发硬件发送数据
    #if (E_MODBUS_DEBUG==1)
    show_emodbus_tx_data(push_last_index);
    #elif (E_MODBUS_DEBUG==0)
    E_MODBUS_TX;
    #endif


    push_last_index_clear();
    emodbus_dec_clear();
}

void emodbus_write_func() {
    uint16_t offset = EModbusDataDec.read_write_reg_start - EModbusDec.read_write_reg_start_addr;
    uint8_t *p = EModbusDataDec.read_write_data_p;
    uint16_t index = 0;
    //写数据
    for (index = offset; index < (offset + EModbusDataDec.read_write_reg_length); index++) {
        eModbusRWReg[index] = *p << 8 | *(p + 1);
        p += 2;
    }

    generate_tx_data();

}

void emodbus_read_func() {
    //读出数据并组装生成回复码
    generate_tx_data();
}

void emodbus_private_func() {
    //执行功能 生成回复码
    generate_tx_data();
}

uint8_t exec_emodbus(uint8_t *data_buff) {
    uint8_t exec_result = FALSE;
    //解析
    exec_result = verify_emodbus_rx_data(data_buff);

    #if (E_MODBUS_DEBUG == 1)
    show_emodbus_verify_data();
    #endif
    //执行 生成返回数据
    if (exec_result == TRUE) {
        switch (EModbusDataDec.func_code) {
            case E_MODBUS_WRITE_FUNC_CODE:
                emodbus_write_func();
                break;
            case E_MODBUS_READ_FUNC_CODE:
                emodbus_read_func();
                break;
            case E_MODBUS_PRAViTE_FUNC_CODE:
                emodbus_private_func();
                break;
        }
    }
    #if (E_MODBUS_DEBUG == 1)
    show_r_w_reg();
    #endif

    return exec_result;
}


void show_emodbus_verify_data() {
    uint8_t *p = EModbusDataDec.read_write_data_p, length = EModbusDataDec.read_write_reg_length * 2;
    printf("\r\nEModbusDataDec\r\n");
    printf("slave_id:%x\r\n", EModbusDataDec.slave_id);
    printf("func_code:%x\r\n", EModbusDataDec.func_code);
    printf("read_write_reg_start:%x\r\n", EModbusDataDec.read_write_reg_start);
    printf("read_write_reg_length:%x\r\n", EModbusDataDec.read_write_reg_length);

    if (EModbusDataDec.func_code == E_MODBUS_WRITE_FUNC_CODE) {
        printf("read_write_data length:%x\r\n", EModbusDataDec.read_write_reg_length);
        for (; length > 0; length--) {
            printf("%x ", *p);
            p++;
        }
        printf("\r\n");
    }
    printf("func_private_code1:%x\r\n", EModbusDataDec.func_private_code1);
    printf("func_private_code2:%x\r\n", EModbusDataDec.func_private_code2);
    printf("crc:%x\r\n", EModbusDataDec.crc);
}




